/*
* polygon_coverage_planning implements algorithms for coverage planning in
* general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous
* Systems Lab, ETH Zürich
*
* This program is free software: you can redistribute it and/or modify it under
* the terms of the GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option) any later
* version.
*
* This program is distributed in the hope that it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
* FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
* details.
*
* You should have received a copy of the GNU General Public License along with
* this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "bcd.h"
#include "decomposition.h"
#include "tcd.h"
#include "weakly_monotone.h"


namespace polygon_coverage_planning {

std::vector<Direction_2> findEdgeDirections(const PolygonWithHoles &pwh) {
    // Get all possible polygon directions. 得到所有可能的多边形方向。
    std::vector<Direction_2> directions;
    for (size_t i = 0; i < pwh.outer_boundary().size(); ++i) {
        directions.push_back(pwh.outer_boundary().edge(i).direction());
    }
    for (auto hit = pwh.holes_begin();
         hit != pwh.holes_end(); ++hit) {
        for (size_t i = 0; i < hit->size(); i++) {
            directions.push_back(hit->edge(i).direction());
        }
    }
    // Remove redundant directions. 删除多余的方向。
    std::set<size_t> to_remove;
    for (size_t i = 0; i < directions.size() - 1; ++i) {
        for (size_t j = i + 1; j < directions.size(); ++j) {
            if (CGAL::orientation(directions[i].vector(), directions[j].vector()) ==
                CGAL::COLLINEAR)
                to_remove.insert(j);
        }
    }
    for (auto rit = to_remove.rbegin();
         rit != to_remove.rend(); ++rit) {
        directions.erase(std::next(directions.begin(), *rit));
    }
    // Add opposite directions. 加上相反的方向。
    std::vector<Direction_2> temp_directions = directions;
    for (auto & temp_direction : temp_directions) {
        directions.push_back(-temp_direction);
    }
    return directions;
}

std::vector<Direction_2> findPerpEdgeDirections(const PolygonWithHoles &pwh) {
    std::vector<Direction_2> directions = findEdgeDirections(pwh);
    for (auto &d : directions) {
        d = Direction_2(-d.dy(), d.dx());
    }
    return directions;
}

double findBestSweepDir(const Polygon_2 &cell, Direction_2 *best_dir) {
    // Get all sweepable edges.
    PolygonWithHoles pwh(cell);
    std::vector<Direction_2> edge_dirs = getAllSweepableEdgeDirections(cell);

    // Find minimum altitude.
    double min_altitude = std::numeric_limits<double>::max();
    for (const auto &dir : edge_dirs) {
        auto s = findSouth(cell, Line_2(Point_2(CGAL::ORIGIN), dir));
        auto n = findNorth(cell, Line_2(Point_2(CGAL::ORIGIN), dir));
        auto orthogonal_vec =
                dir.vector().perpendicular(CGAL::Orientation::POSITIVE);
        Line_2 line_through_n(*n, orthogonal_vec.direction());
        auto s_proj = line_through_n.projection(*s);
        double altitude =
                std::sqrt(CGAL::to_double(CGAL::squared_distance(*n, s_proj)));

        if (altitude < min_altitude) {
            min_altitude = altitude;
            if (best_dir) *best_dir = dir;
        }
    }

    return min_altitude;
}

bool computeBestBCDFromPolygonWithHoles(const PolygonWithHoles &pwh, std::vector<Polygon_2> *bcd_polygons) {
    bcd_polygons->clear();
    double min_altitude_sum = std::numeric_limits<double>::max();
    // Get all possible decomposition directions. 得到所有可能的分解方向。
    std::vector<Direction_2> directions = findPerpEdgeDirections(pwh);
    // For all possible rotations: 对于所有可能的旋转:
    for (const auto &dir : directions) {
        // Calculate decomposition. 计算分解。
        std::vector<Polygon_2> cells = computeBCD(pwh, dir);
        // Calculate minimum altitude sum for each cell. 计算每个单元的最小高度和。
        double min_altitude_sum_tmp = 0.0;
        for (const auto &cell : cells) {
            min_altitude_sum_tmp += findBestSweepDir(cell);
        }
        // Update best decomposition. 更新最佳分解。
        if (min_altitude_sum_tmp < min_altitude_sum) {
            min_altitude_sum = min_altitude_sum_tmp;
            *bcd_polygons = cells;
        }
    }
    return !bcd_polygons->empty();
}

bool computeBestTCDFromPolygonWithHoles(const PolygonWithHoles &pwh,
                                        std::vector<Polygon_2> *tcd_polygons) {
    tcd_polygons->clear();
    double min_altitude_sum = std::numeric_limits<double>::max();
    // Get all possible decomposition directions. 得到所有可能的分解方向。
    std::vector<Direction_2> directions = findPerpEdgeDirections(pwh);
    // For all possible rotations: 对于所有可能的旋转:
    for (const auto &dir : directions) {
        // Calculate decomposition.  计算分解。
        std::vector<Polygon_2> cells = computeTCD(pwh, dir);
        // Calculate minimum altitude sum for each cell. 计算每个单元的最小高度和。
        double min_altitude_sum_tmp = 0.0;
        for (const auto &cell : cells) {
            min_altitude_sum_tmp += findBestSweepDir(cell);
        }
        // Update best decomposition. 更新最佳分解。
        if (min_altitude_sum_tmp < min_altitude_sum) {
            min_altitude_sum = min_altitude_sum_tmp;
            *tcd_polygons = cells;
        }
    }
    return !tcd_polygons->empty();
}

}  // namespace polygon_coverage_planning
